#!/usr/bin/env python
# -*- coding: utf-8 -*-
import sys
import rospy
from nav_and_digging.srv import BaseNav

import serial
import pynmea2
import time

import math
Pi = 3.1415926
Latitude = 0
Longitude = 0
Heading = 0
Time = 0

LatitudeList = []
LongitudeList = []
HeadingList = []

def GpsNav_client(x,y,w):

    try:
        GpsNav_client = rospy.ServiceProxy('/BaseNav_Server', BaseNav)

		# call service
        response = GpsNav_client(x, y, w)
        return response.result
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e

def getDistanceByGps(lat1, lng1, lat2, lng2):

    EarthRadius = 6378137
    Rad = Pi / 180.0

    radlat1 = lat1 * Rad
    radlat2 = lat2 * Rad
    a = radlat1 - radlat2
    b = (lng1 - lng2)*Rad
    s = 2*math.asin( \
          math.sqrt(math.pow(math.sin(a/2),2)+ \
          math.cos(radlat1)*math.cos(radlat2)*math.pow(math.sin(b/2),2)) \
          )
    s = s*EarthRadius
    s = (s * 10000) / 10000

    return s



def getAngleByGps(lat1, lng1, lat2, lng2):
    
    x = math.sin(lng2 - lng1) * math.cos(lat2)
    y = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(lng2 - lng1)
    angle = math.atan2(x,y) * 180 / Pi

    if angle<0:
        angle = angle+360

    return angle


def GpsUpdate():
    global Heading, Latitude, Longitude, Time
    ser.flushInput()
    while True:
        recv1 = ser.readline().decode()
        if recv1.startswith('$GNHDT'):
            print('1.$GNHDT Received.')
            record1 = pynmea2.parse(recv1)
            Heading = float(record1.heading)
            print('--------------------------------')
            print('Heading:', Heading)

            recv2 = ser.readline().decode()
            if recv2.startswith('$GPGGA'):
                print('2.$GPGGA Received.')
                record2 = pynmea2.parse(recv2)
                print('Number of Satellites availabe:', record2.num_sats)

                recv3 = ser.readline().decode()
                if recv3.startswith('$GPRMC'):
                    print('3.$GPRMC Received.')
                    record3 = pynmea2.parse(recv3)
                    Latitude = float(record3.latitude)
                    Longitude = float(record3.longitude)
                    Time = str(record3.timestamp)

                    print('Fix Status: ', record3.status)
                    print('Latitude: ', Latitude)
                    print('Longitude: ', Longitude)
                    print('Time: ', Time)
                    break

        #     recv1 = ser.readline().decode()
        #         if recv.startswith('$GNHDT'):
        #             print('--------------------------------')
        #             # print('GNHDT record:', record)
        #             print('Heading:', float(record.heading))
        #         elif recv.startswith('$GPGGA') or recv.startswith('$GNGGA'):
        #             # print('GPGGA record:', record)
        #             print('Number of Satellites availabe:', record.num_sats)
        #         elif recv.startswith('$GPRMC') or recv.startswith('$GNRMC'):
        #             # print('GPRMC record:', record)
        #             print('Fix Status: ', record.status)
        #             print('Latitude: ', float(record.latitude))
        #             print('Longitude: ', float(record.longitude))
        #             print('Time: ', str(record.timestamp))
        #             break
        # except pynmea2.nmea.ParseError:
        #     print('NMEA wrong！')



def WayPointsParse(filename):
    with open(filename, 'r') as file_to_read:
        while True:
            lines = file_to_read.readline() # 整行读取数据
            if not lines:
                break
            Latitude_tmp, Longitude_tmp, Heading_tmp = [float(i) for i in lines.split()] # 将整行数据分割处理，如果分割符是空格，括号里就不用传入参数，如果是逗号， 则传入‘，'字符。
            LatitudeList.append(Latitude_tmp)
            LongitudeList.append(Longitude_tmp)  # 添加新读取的数据
            HeadingList.append(Heading_tmp)
        # pos = np.array(pos) # 将数据从list类型转换为array类型。
        # Efield = np.array(Efield)
    print("LatitudeList: ", LatitudeList)
    print("LongitudeList: ", LongitudeList)
    print("HeadingList: ", HeadingList)

if __name__ == "__main__":
    
    ################GpsNav_client init#######################
    print("---GpsNav_client init...")
    rospy.init_node('GpsNav_client')

    port_name = rospy.get_param('~port','/dev/ttyUSB2')
    baud = int(rospy.get_param('~baud','115200'))

    ser = serial.Serial(port_name, baud, timeout=5)

	# Wait for/BaseNav_Server 
    rospy.wait_for_service('/BaseNav_Server')

    ################WayPoint File Parse#######################
    print("---WayPoint File Parse...")
    WayPointsParse('/home/shine/Noah_ws/src/nav_and_digging/scripts/WayPoints.txt')
    print("WayPoints num: ", len(LatitudeList))

    ################NavAndDrill#######################
    for i in range(len(LatitudeList)):
        print('WayPoint: ', i)

        #####################Transform########################
        print("---Transform...")

        # ################GPS Parse Update######################
        # print("---GPS Parse Update...")
        # GpsUpdate()
        # AimLatitude =  LatitudeList[i]
        # AimLongitude = LongitudeList[i]
        # AimHeading = HeadingList[i]
        # ################GPS Parse Update######################

        ################FakeGPS Parse Update##################
        if i + 1 == len(LatitudeList):
            break
        print("---FakeGPS Parse Update...")
        Latitude =  LatitudeList[i]
        Longitude = LongitudeList[i]
        Heading = HeadingList[i]

        AimLatitude =  LatitudeList[i + 1]
        AimLongitude = LongitudeList[i + 1]
        AimHeading = HeadingList[i + 1]
        ################FakeGPS Parse Update##################


        print("Latitude: ", Latitude)
        print("Longitude: ", Longitude)
        print("Heading: ", Heading)
        print("Aim Latitude: ", AimLatitude)
        print("Aim Longitude: ", AimLongitude)
        print("Aim Heading: ", AimHeading)
        
        Distance = getDistanceByGps(Latitude, Longitude, AimLatitude, AimLongitude)
        GeoAngle = getAngleByGps(Latitude, Longitude, AimLatitude, AimLongitude)
        print("Distance: ", Distance)
        print("GeoAngle: ", GeoAngle)

        delta_x = Distance * math.cos((GeoAngle - Heading) * Pi / 180)
        delta_y = -Distance * math.sin((GeoAngle - Heading) * Pi / 180)
        delta_theta = AimHeading - Heading

        print("delta_x: ", delta_x)
        print("delta_y: ", delta_y)
        print("delta_theta: ", delta_theta)

        ##################service client############ 
        # # call service
        print("---call service...")
        print "Show calling result : %s" %(GpsNav_client(delta_x, delta_y, delta_theta))


    '''
    #####################TEST#######################
    ################GpsNav_client init#######################
    print("---GpsNav_client init...")
    rospy.init_node('GpsNav_client')
	# Wait for/BaseNav_Server 
    rospy.wait_for_service('/BaseNav_Server')

    # Latitude = 31.2285708713
    # Longitude = 121.4003952034
    # Heading = 0
    # AimLatitude = 31.2286305053
    # AimLongitude = 121.4004810341
    # AimHeading = 45

    
    Latitude = 31.2286305053
    Longitude = 121.4004810341
    Heading = 45
    AimLatitude = 31.2287268371
    AimLongitude = 121.4003952034
    AimHeading = -45
    

    print("---GpsNav_client init...")
    rospy.init_node('GpsNav_client')
    # Wait for/BaseNav_Server 
    rospy.wait_for_service('/BaseNav_Server')

    Distance = getDistanceByGps(Latitude, Longitude, AimLatitude, AimLongitude)
    GeoAngle = getAngleByGps(Latitude, Longitude, AimLatitude, AimLongitude)
    print("Distance: ", Distance)
    print("GeoAngle: ", GeoAngle)

    delta_x = Distance * math.cos((GeoAngle - Heading) * Pi / 180)
    delta_y = -Distance * math.sin((GeoAngle - Heading) * Pi / 180)
    delta_theta = AimHeading - Heading

    print("delta_x: ", delta_x)
    print("delta_y: ", delta_y)
    print("delta_theta: ", delta_theta)

    ##################service client############
    # # call service
    print("---call service...")
    print "Show calling result : %s" %(GpsNav_client(delta_x, delta_y, delta_theta))
    '''
